/*
 * get_mass.c
 *
 *  Created on: Apr 26, 2010
 *      Author: marchi
 */

#include "stdio.h"
#include "stdlib.h"
#include "statutil.h"
#include "typedefs.h"
#include "smalloc.h"
#include "vec.h"
#include "copyrite.h"
#include "statutil.h"
#include "tpxio.h"
#include "fftgrid.h"
#include "index.h"
#include "pbc.h"
#include "math.h"
#include "get_mass.h"

#define VERTEX 8
#define UNITCM		(1.0E-8)
#define AVOGADRO	 (6.0221367e23)

const rvec cube[VERTEX]={{0,0,0},{1,0,0},
		{0,1,0},{0,0,1},{1,1,0},
		{1,0,1},{0,1,1},{1,1,1}};

inline static int do_int_pbc(int tx, int nx){
	if(!(tx % nx)) return 0;
	float rtx= (float) tx/ (float) nx;
	rtx=rtx-rint(rtx-0.5);
	int my= (int) (rtx*(float) nx);
	if(my < 0 || my > nx-1) {
		printf(" Abort \n");
		abort();
	}
	return my;
}
inline static int do_int0_pbc(int tx, int nx){
	return (nx > tx) ? tx : tx - nx;
}

static real TriDiff(real g, real cube){
	return((g-cube>=0.0)? cube-g+1 : g-cube+1);
}
static void TriLinear(real gx, real gy, real gz, real q, real *qq){
	int n;
	real dd;
	real sum=0;
	for(n=0;n<VERTEX;n++){
		dd=TriDiff(gx,cube[n][XX])*TriDiff(gy,cube[n][YY])*TriDiff(gz,cube[n][ZZ]);
		qq[n]=dd*q;
		sum+=qq[n];
	}
}

int get_mass(real*** Rho_slt, real*** Rho_sol,real dvol,
		real* mass,int nx,int ny,int nz,int nind, atom_id *cindex, t_trxframe* fr){

	real** xa;
	matrix oc,co;
	FILE *fp;
	fp=fopen("testa.dat","w+");
	for(int n=0;n<DIM;n++){
		for(int m=0;m<DIM;m++){
			co[m][n]=fr->box[n][m];
		}
	}
	real fact=1.0/(AVOGADRO*pow(UNITCM,3)*dvol);
	calc_recipbox(co,oc);
	int natoms=fr->natoms;
	real x1,y1,z1,r1,s1,t1,gx,gy,gz;
	int mx,my,mz;
	real qq[8];


	int ox,oy,oz,tx,ty,tz;

	xa = malloc(sizeof(real *) * natoms);
	for (int i = 0; i < natoms; i++) {
		xa[i] = malloc(sizeof(real) * DIM);
	}
	for (int i = 0; i < natoms; i++) {
		xa[i][XX] = oc[XX][XX] * fr->x[i][XX] + oc[XX][YY] * fr->x[i][YY] + oc[XX][ZZ] * fr->x[i][ZZ];
		xa[i][YY] = oc[YY][XX] * fr->x[i][XX] + oc[YY][YY] * fr->x[i][YY] + oc[YY][ZZ] * fr->x[i][ZZ];
		xa[i][ZZ] = oc[ZZ][XX] * fr->x[i][XX] + oc[ZZ][YY] * fr->x[i][YY] + oc[ZZ][ZZ] * fr->x[i][ZZ];
	}
	for (int i = 0; i < natoms; i++) {
		x1 = xa[i][XX];
		y1 = xa[i][YY];
		z1 = xa[i][ZZ];
		r1 = (real) nx * (x1 - rint(x1 - 0.5));
		s1 = (real) ny * (y1 - rint(y1 - 0.5));
		t1 = (real) nz * (z1 - rint(z1 - 0.5));
		mx = (int) r1;
		my = (int) s1;
		mz = (int) t1;
		gx = r1 - (real) mx;
		gy = s1 - (real) my;
		gz = t1 - (real) mz;
		TriLinear(gx, gy, gz, mass[i], qq);
		for (int n = 0; n < VERTEX; n++) {
			tx = (int) cube[n][XX];
			ty = (int) cube[n][YY];
			tz = (int) cube[n][ZZ];
			ox=do_int0_pbc(mx+tx,nx);
			oy=do_int0_pbc(my+ty,ny);
			oz=do_int0_pbc(mz+tz,nz);
			Rho_slt[ox][oy][oz] += qq[n]*fact;
		}
	}

	for (int ii=0; ii < nind; ii++) {
		int i=cindex[ii];
		x1 = xa[i][XX];
		y1 = xa[i][YY];
		z1 = xa[i][ZZ];
		r1 = (real) nx * (x1 - rint(x1 - 0.5));
		s1 = (real) ny * (y1 - rint(y1 - 0.5));
		t1 = (real) nz * (z1 - rint(z1 - 0.5));
		mx = (int) r1;
		my = (int) s1;
		mz = (int) t1;
		gx = r1 - (real) mx;
		gy = s1 - (real) my;
		gz = t1 - (real) mz;

		TriLinear(gx, gy, gz, mass[i], qq);

		for (int n = 0; n < VERTEX; n++) {
			tx = (int) cube[n][XX];
			ty = (int) cube[n][YY];
			tz = (int) cube[n][ZZ];
			ox=do_int0_pbc(mx+tx,nx);
			oy=do_int0_pbc(my+ty,ny);
			oz=do_int0_pbc(mz+tz,nz);
			Rho_sol[ox][oy][oz]  += qq[n]*fact;
			Rho_slt[ox][oy][oz]  -= qq[n]*fact;
		}
 	}
	for (int i = 0; i < natoms; i++) {
		free(xa[i]);
	}
	free(xa);
	fclose(fp);
	return 0;
}
